#include "EndFaceCloud.h"
#include "PipesSetCloud.h"
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/boundary.h>
#include <pcl/common/distances.h>
#include "CloudOperation.h"

EndFaceCloud::EndFaceCloud() {
}

EndFaceCloud::EndFaceCloud(const PointCloud<PointXYZ>::Ptr& cloud) {
    this->points = cloud->points;
    this->header = cloud->header;
    this->width = cloud->width;
    this->height = cloud->height;
    this->is_dense = cloud->is_dense;
}

EndFaceCloud::EndFaceCloud(const PointCloud<PointXYZ>& cloud) {
    this->points = cloud.points;
    this->header = cloud.header;
    this->width = cloud.width;
    this->height = cloud.height;
    this->is_dense = cloud.is_dense;
}

EndFaceCloud::EndFaceCloud(const EndFaceCloud& other) {
    this->points = other.points;
    this->header = other.header;
    this->width = other.width;
    this->height = other.height;
    this->is_dense = other.is_dense;
    //this->normal_cloud = other.normal_cloud;
}

void EndFaceCloud::Display() {
    pcl::PointCloud<PointXYZ>::Ptr pCloud;
    pCloud = std::make_shared<pcl::PointCloud<PointType>>(*this);
#define VIEW_CLOUD1 pCloud
    //#define VIEW_CLOUD2 RealCloudLeft
    //#define VIEW_CLOUD3 transformed
    pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
    // 添加坐标轴
#ifndef UNIT_CLOUD
    viewer.addCoordinateSystem(0.1); // 参数表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#else
    viewer.addCoordinateSystem(0.1 * UNIT_CLOUD); // 参数1.0表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#endif
    // 添加点云到viewer中，命名为"cloud"
    viewer.addPointCloud<PointType>(VIEW_CLOUD1, "cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointType>(VIEW_CLOUD2, "cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointType>(VIEW_CLOUD3, "cloud3");
#endif
    // 创建一个自定义颜色处理器，将点云颜色设置为红色
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler1(VIEW_CLOUD1, 255, 0, 0);//红
#ifdef VIEW_CLOUD2
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler2(VIEW_CLOUD2, 0, 255, 0);//绿
#endif
#ifdef VIEW_CLOUD3
    pcl::visualization::PointCloudColorHandlerCustom<PointType> colorHandler3(VIEW_CLOUD3, 0, 0, 255);//蓝
#endif
    // 添加带颜色的点云到viewer中，命名为"colored cloud"
    viewer.addPointCloud<PointType>(VIEW_CLOUD1, colorHandler1, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointType>(VIEW_CLOUD2, colorHandler2, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointType>(VIEW_CLOUD3, colorHandler3, "colored cloud3");
#endif
    // 设置点云的渲染属性，点的大小为1
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud3");
#endif
    // 设置viewer的背景颜色为灰色
    viewer.setBackgroundColor(100, 100, 100);
    //主循环
    while (!viewer.wasStopped()) {
        // 更新viewer
        viewer.spinOnce(100);
    }
}

int EndFaceCloud::EdgeExtraction(TorusEdgeCloud& edgeCloud) const {
    if (!edgeCloud.points.empty()) {
        edgeCloud.clear();
    }
    auto InputCloud = std::make_shared<EndFaceCloud>(*this);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Boundary>::Ptr cloud_boundary(new pcl::PointCloud<pcl::Boundary>);

    // 创建体素网格滤波器
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(InputCloud);
    voxel_grid.setLeafSize(0.003f, 0.003f, 0.003f); // 设置体素大小
    voxel_grid.filter(*cloud_filtered);
    
    //pcl::PointCloud<pcl::Normal>::Ptr cloud_normals = std::make_shared<pcl::PointCloud<pcl::Normal>>(InputCloud->normal_cloud);
    
    // 计算法线
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud_filtered);
    
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(0.018); // 设置搜索半径
    ne.compute(*cloud_normals);

    // 检测边界
    pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> be;
    be.setInputCloud(cloud_filtered);
    be.setInputNormals(cloud_normals);
    be.setSearchMethod(tree);
    
    be.setRadiusSearch(3); // 设置搜索半径
    be.compute(*cloud_boundary);

    // 提取边界点
    EndFaceCloud filteredCloud = *cloud_filtered;
    filteredCloud.Display();
    for (size_t i = 0; i < cloud_boundary->points.size(); ++i) {
        if (cloud_boundary->points[i].boundary_point) {
            edgeCloud.points.push_back(cloud_filtered->points[i]);
        }
    }

    //判断提取到的点云是否正常

    return 1;
}


//*********************************************************************************//

TorusEdgeCloud::TorusEdgeCloud() {
    this->center_x = 0;
    this->center_y = 0;
    this->center_z = 0;
    //this->radius_a = 0;
    //this->radius_b = 0;
    this->angle_a = 0;
    this->angle_b = 0;
    this->angle_c = 0;
}

TorusEdgeCloud::TorusEdgeCloud(const PointCloud<PointXYZ>::Ptr& cloud) {
    this->points = cloud->points;
    this->header = cloud->header;
    this->width = cloud->width;
    this->height = cloud->height;
    this->is_dense = cloud->is_dense;
    this->center_x = 0;
    this->center_y = 0;
    this->center_z = 0;
    //this->radius_a = 0;
    //this->radius_b = 0;
    this->angle_a = 0;
    this->angle_b = 0;
    this->angle_c = 0;
}

TorusEdgeCloud::TorusEdgeCloud(const PointCloud<PointXYZ>& cloud) {
    this->points = cloud.points;
    this->header = cloud.header;
    this->width = cloud.width;
    this->height = cloud.height;
    this->is_dense = cloud.is_dense;
    this->center_x = 0;
    this->center_y = 0;
    this->center_z = 0;
    //this->radius_a = 0;
    //this->radius_b = 0;
    this->angle_a = 0;
    this->angle_b = 0;
    this->angle_c = 0;
}

TorusEdgeCloud::TorusEdgeCloud(const TorusEdgeCloud& other) {
    this->points = other.points;
    this->header = other.header;
    this->width = other.width;
    this->height = other.height;
    this->is_dense = other.is_dense;
    this->center_x = other.center_x;
    this->center_y = other.center_y;
    this->center_z = other.center_z;
    //this->radius_a = other.radius_a;
    //this->radius_b = other.radius_b;
    this->angle_a = other.angle_a;
    this->angle_b = other.angle_b;
    this->angle_c = other.angle_c;
}

void TorusEdgeCloud::Display() {
    CloudOperation CloudOperator;
    pcl::PointCloud<PointXYZ>::Ptr pCloud;
    pcl::PointCloud<PointXYZ>::Ptr pCircleCloud = std::make_shared<pcl::PointCloud<PointXYZ>>();
    CloudOperator.CreateCircle3DCloud(this->coefficients, *pCircleCloud);
    pCloud = std::make_shared<pcl::PointCloud<PointXYZ>>(*this);
#define VIEW_CLOUD1 pCloud
#define VIEW_CLOUD2 pCircleCloud
    //#define VIEW_CLOUD3 transformed
    pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
    // 添加坐标轴
#ifndef UNIT_CLOUD
    viewer.addCoordinateSystem(0.1); // 参数表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#else
    viewer.addCoordinateSystem(0.1 * UNIT_CLOUD); // 参数1.0表示坐标轴的长度，单位：mm,红色为x轴,绿色为y轴,蓝色为z轴
#endif
    // 添加点云到viewer中，命名为"cloud"
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD1, "cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD2, "circle");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD3, "cloud3");
#endif
    // 创建一个自定义颜色处理器，将点云颜色设置为红色
    pcl::visualization::PointCloudColorHandlerCustom<PointXYZ> colorHandler1(VIEW_CLOUD1, 255, 0, 0);//红
#ifdef VIEW_CLOUD2
    pcl::visualization::PointCloudColorHandlerCustom<PointXYZ> colorHandler2(VIEW_CLOUD2, 0, 255, 0);//绿
#endif
#ifdef VIEW_CLOUD3
    pcl::visualization::PointCloudColorHandlerCustom<PointXYZ> colorHandler3(VIEW_CLOUD3, 0, 0, 255);//蓝
#endif
    // 添加带颜色的点云到viewer中，命名为"colored cloud"
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD1, colorHandler1, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD2, colorHandler2, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.addPointCloud<PointXYZ>(VIEW_CLOUD3, colorHandler3, "colored cloud3");
#endif
    // 设置点云的渲染属性，点的大小为1
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud1");
#ifdef VIEW_CLOUD2
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud2");
#endif
#ifdef VIEW_CLOUD3
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "colored cloud3");
#endif
    // 设置viewer的背景颜色为灰色
    viewer.setBackgroundColor(100, 100, 100);
    //主循环
    while (!viewer.wasStopped()) {
        // 更新viewer
        viewer.spinOnce(100);
    }
}

int TorusEdgeCloud::TorusFit() {
    //pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::ExtractIndices<pcl::PointXYZ> extract;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>(*this));
    TorusEdgeCloud::Ptr CurRing = std::make_shared<TorusEdgeCloud>(*cloud);

    // 设置分割参数
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CIRCLE3D);
    seg.setMethodType(SAC_LMEDS);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.05);

    // 输入点云
    seg.setInputCloud(cloud);

    // 执行分割
    seg.segment(*inliers, this->coefficients);

    if (inliers->indices.size() == 0) {
        return -1;
    }

    this->center_x = this->coefficients.values[0];
    this->center_y = this->coefficients.values[1];
    this->center_z = this->coefficients.values[2];
    //this->radius_a = coefficients->values[6];
    //this->radius_b = coefficients->values[7];
    this->angle_a = this->coefficients.values[4];
    this->angle_b = this->coefficients.values[5];
    this->angle_c = this->coefficients.values[6];

    return 1;
}

// 判断形状是否正常，不正常返回0
bool TorusEdgeCloud::IsNormalShape() const {
    // 创建一个搜索树计算各点到质心的距离
    PointCloud<PointXYZ>::Ptr pInputCloud(new PointCloud<PointXYZ>(*this));
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(pInputCloud);

    // 计算质心
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*this, centroid);

    // 计算各点到质心的距离
    std::vector<float> distances;
    for (const auto& point : *pInputCloud) {
        float distance = pcl::euclideanDistance(point, pcl::PointXYZ(centroid[0], centroid[1], centroid[2]));
        distances.push_back(distance);
    }

    // 找到距离的最大值和最小值
    float maxDistance = *std::max_element(distances.begin(), distances.end());
    float minDistance = *std::min_element(distances.begin(), distances.end());

    // 这里可以根据最大值和最小值来判断形状是否正常
    // 例如，如果最大值和最小值的差距过大，可能表示形状不正常
    if (maxDistance > 3 * minDistance) {
        return false;
    }

    return true;
}
